import socket
from statistics import mode
import struct
import sys
from time import time
from ars_parse_detection import ArsParseDetection
from ars_parse_object import ArsParseObject
from datetime import datetime
import os
debug_frame_flag = False
online_mode = True
def create_dir(path):
        isExists = os.path.exists(path)
        if not isExists:
            os.makedirs(path)
            print('create folder :' + path)
        else:
            print('the folder already exists' + '(%s)' % (path))
          
def data_save(dir,data):
    if(debug_frame_flag == True):
        dir_name = './debug_'+dir
        create_dir(dir_name)
        now = datetime.now()
        file_name = './debug_'+dir + '/debug_frame' + now.strftime('%Y%m%d%H%M%S') + '.bin'
        f = open(file_name,'wb')
        f.write(data)
def data_read(dir,name):
    file_name = './debug_'+dir + '/'+name + '.bin'
    f = open(file_name,'rb')
    return f.read()

if __name__=="__main__":
    if(online_mode == True):

        multicast_group = '224.0.2.2'
        server_address = ('',42102)

        # Create the socket
        sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        sock.settimeout(1)

        # Bind to the server address
        sock.bind(server_address)
        # Tell the operating system to add the socket to the multicast group
        # on all interfaces.
        group = socket.inet_aton(multicast_group)
        mreq = struct.pack('4sL', group, socket.INADDR_ANY)
        sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
        print("socket start,waitting msg")
    print("start read 1000 times")
    for i in range(1000):
        if(online_mode == True):
        # Receive/respond loop
            try:
                data, address = sock.recvfrom(50000)
            except Exception as e:
                print("无法从ARS548接收消息,timeout,请检查")
                continue
        else:
            data = data_read('object','debug_frame20220317163349')
        data_length = len(data)
        # print(data_length)
        # print()
        if(data_length==35336):
            # f = open("detec_frame",'w')
            # f.write(data)
            # print('there')
            ars_detection = ArsParseDetection.from_bytes(data)
            print(">>>>>>>>>>>>")
            print(ars_detection.header.service_id)
            print(ars_detection.header.method_id)
            print(ars_detection.header.length)
            print(ars_detection.header.client_id)
            print(ars_detection.header.session_id)
            print(ars_detection.header.protocol_version)
            print(ars_detection.header.interface_version)
            print(ars_detection.header.message_type)
            print(ars_detection.header.return_code)
            print(len(ars_detection.body.list_detections))
            print(ars_detection.body.origin_yawstd)
            print(ars_detection.body.crch)
            print(ars_detection.body.crcl)
            print('body.data_id',ars_detection.body.data_id)

            for id,d in enumerate(ars_detection.body.list_detections):
                # print("f_azimuthangle",id,d.f_azimuthangle)
                # if(d.f_range > 0):
                print("f_azimuthangle",id,d.f_azimuthangle)
                print("u_objectid",id,d.u_objectid)
                    # print("u_classification",id,d.u_classification)
                    
                    # print("s_rcs",id,d.s_rcs)
            data_save('detection',data)    
            print("<<<<<<<<<<<<<")
            # print(ars_detection.body.list_detections[0].f_azimuthanglestd)
        elif(data_length==9401):
            ars_object = ArsParseObject.from_bytes(data)
            print('method_id',ars_object.header.method_id)
            print(ars_object.header.length)
            print(ars_object.header.protocol_version)

            print('u_shape_width_edge_std',ars_object.body.objectlist_objects[0].u_shape_width_edge_std)
        
            data_save('object',data) 

        elif(data_length==158):
            data_save('landmarks',data) 
        elif(data_length==803):
            data_save('fieldofview',data) 
        elif(data_length==50):
            data_save('ego_speed',data)
        else:
            print("other data") 


    # filename = 'test_text.txt'
    # with open(filename, 'w') as file_object:
    #     file_object.write(data.decode('gbk'))
    # print(">>>>>")
    # while True:
    #     sock.send("2323")